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Abstract — This paper presents a novel approach to the 
design of globally asymptotically stable (GAS) position filters 
for Autonomous Underwater Vehicles (AUVs) based directly 
on the nonlinear sensor readings of an Ultra-short Baseline 
(USBL) and a Doppler Velocity Log (DVL). Central to the 
proposed solution is the derivation of a linear time-varying 
(LTV) system that fully captures the dynamics of the nonlinear 
system, allowing for the use of powerful linear system analysis 
and filtering design tools that yield GAS filter error dynamics. 
Simulation results reveal that the proposed filter is able to 
achieve the same level of performance of more traditional 
solutions, such as the Extended Kalman Filter (EKF), while 
providing, at the same time, GAS guarantees, which are absent 
for the EKF. 

I. Introduction 

The design and implementation of navigation systems 
stands out as one of the most critical steps towards the 
successful operation of autonomous vehicles. The quality of 
the overall estimates of the navigation system dramatically 
influences the capability of the vehicles to perform precision- 
demanding tasks, see [1] and [2] for interesting and detailed 
surveys on underwater vehicle navigation and its relevance. 
This paper presents a novel approach to the design of 
globally asymptotically stable (GAS) position filters directly 
based on the nonlinear sensor readings. 

Consider an underwater vehicle equipped with an Ultra- 
short Baseline (USBL) underwater positioning device, a triad 
of orthogonally mounted rate gyros, and a Doppler Velocity 
Log (DVL), that moves in the presence of unknown ocean 
currents in a scenario that has a fixed transponder, as depicted 
in Fig. [T] The USBL is composed of a small calibrated array 
of acoustic receivers, and measures the distance between the 
transponder and the receivers installed on-board. Given the 
proximity of the sensors in the receiving array, hence the 
name Ultra-Short Baseline (USBL), the USBL is capable of 
measuring more accurately the Range-Difference-of-Arrival 
(RDOA) of the acoustic waves at the receivers compared 
to the actual distances between the transponder and all the 
receivers. The DVL measures the velocity of the vehicle 
with respect to the fluid, and the rate gyros measure the 
angular velocities of the vehicle. Due to noisy measurements, 
unknown ocean currents, and the nonlinear nature of the 
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Fig. 1. Mission scenario 

range measurements, a filtering solution is required in order 
to correctly estimate the position of the transponder in the 
vehicle coordinate frame and the inertial velocity of the 
vehicle. Recent advances in the area of underwater navigation 
based on merging the information from acoustic arrays and 
other inertial sensors like DVLs, can be found in [3], [4], [5] 
and references therein. 

Traditional solutions resort either to the well known 
Extended Kalman Filter (EKF) [6], Particle Filters (PF)[3], 
which lack global asymptotic stability properties, or to 
filtering solutions that use a precomputed position fix from 
the USBL device using the range and bearing and elevation 
angles of the transponder. The computation of this position 
fix often resorts to a Planar-Wave approximation of the 
acoustic wave arriving at the receiving array, previously 
used by the authors [6]. In that case, the error cannot 
be guaranteed to converge to zero due to the planar-wave 
approximation. In fact, the error converges to a neighborhood 
of the origin, not arbitrarily small, that depends on the planar- 
wave approximation, and that only vanishes as the distance 
between the transponder and the vehicle approaches infinity. 
This behavior is obviously undesirable if the vehicle is to 
perform docking or other manoeuvres in the vicinity of a 
transponder, for instance. 

The main contribution of this paper is the design of a 
globally asymptotically stable sensor-based filter to estimate 
the position of the transponder in the vehicle frame and the 
unknown current that biases the DVL readings. The solution 
presented in the paper departs from previous approaches 
as the range measurements are directly embedded in the 
filter structure, thus avoiding the planar-wave approximation, 
and follows related work found in [7], where single range 
measurements were considered and persistent excitation con- 
ditions were imposed on the vehicle motion to bear the 
system observable. In this paper the framework is extended to 
the case of having an array of receivers installed on-board the 
vehicle, which allows for the analysis of the overall system 



without any restriction on the vehicle motion. The nonlinear 
system dynamics are considered to their full extent and no 
linearizations are carried out whatsoever. At the core of the 
proposed filtering framework is the derivation of a linear 
time-varying (LTV) system that captures the dynamics of 
the nonlinear system. The LTV model is achieved through 
appropriate state augmentation, which is shown to mimic 
the nonlinear system, and ultimately allows for the use of 
powerful linear system analysis and filtering design tools that 
yield a novel estimation solution with GAS error dynamics. 
Although outside the scope of this work, the addition of 
an Attitude and Heading Reference System (AHRS) would 
allow for the final solution to be expressed directly in inertial 
coordinates. 

The paper is organized as follows: Section [II] sets the 
problem framework and definitions. The proposed filter 
design and main contributions of the paper are presented 



in Section III where the filter structure is brought to full 
detail and an extensive observability analysis is carried 
out. Simulation results and performance comparison with 



traditional solutions are discussed in Section IV and finally 
Section [V] provides some concluding remarks. 

II. Problem framework 

In order to set the design framework, let {/} denote an 
inertial reference coordinate frame and {B} a coordinate 
frame attached to the vehicle, usually denominated as body- 
fixed coordinate frame. The position of the transponder 
r(t) £ W 3 in the vehicle coordinate frame {B} is given 
by 

r(t)=ll T (t)(s-p(t)), (1) 

where s <E M 3 is the position of the transponder in inertial 
coordinates, p(t) £ K 3 is the position of the vehicle in 
inertial coordinates, and lZ(t) £ SO(3) is the rotation matrix 
from {B} to {/} . The time derivative of lZ(t) verifies 
K(t) = K{t)S (w(t)), where u(t) £ R 3 is the angular 
velocity of {B} with respect to {1} expressed in body-fixed 
coordinates, and S (u>(t)) is the skew-symmetric matrix that 
represents the cross product such that S (lu) a = oj x a. 
Differentiating ([TJ in time yields 



r(f) = -S( W (t))r(()-v(t), 



(2) 



where v(t) £ M 3 is the vehicle velocity expressed in body- 
fixed coordinates. The readings of the DVL are modeled by 



(3) 



where v r (t) £ M 3 is the velocity reading provided by the 
DVL, and J v c (i) £ K 3 is the current velocity expressed in 
inertial coordinates and considered to be constant, that is, 
7 v c (£) = 0. Using the current velocity expressed in body- 
fixed coordinates v c (i) = 7?. T (t) / v c (t) together with Q in 
<|2j yields 



The distances between the transponder and the receivers 
installed on-board the vehicle (as measured by the USBL) 
can be written as 



ft (t)=||b,-r(i)||, i = l, 



,n r 



(5) 



where b ; £ M 3 denotes the position of the receiver in {B} , 
and n r is the number of receivers on the USBL. Combining 
the time-derivative of v c (t) with Q and <|5j yields the 
nonlinear system 

= -5(w(i))r(*)-v.(t)-v r (t), 
= -5(w(t))v„(t), (6) 
= ||bi - r(f)||, i = l,...,n r . 

The problem addressed in this paper is the design of a 
filter for (|6]l considering noisy measurements. 

III. Filter design 

In this section the main results and contributions of the 
paper are presented. In order to reduce the complexity of 
the system dynamics a Lyapunov state transformation is 
firstly introduced in Section III-A The LTV system that 




will mimic the nonlinear behavior of the original system |6]) 
is proposed in Section III-B by means of an appropriate 
state augmentation. The observability analysis of the LTV 
system and its relation with the original nonlinear system is 



conducted in Section III-C and finally in Section III-D the 



design of a Kalman filter is proposed in a stochastic setting 
for the resulting system. 

A. State transformation 

Consider the following state transformation 



x x (t)' 
x 2 (t) 



T(t) 



r(i)' 
v.(t) 



(7) 



r(f) = -S(o;(t))r(t)-v c (t)-v r (i). 



(4) 



where T(i) := diag (TZ(t), TZ(t)) is a Lyapunov state trans- 
formation which preserves all observability properties of the 
original system [8]. 

The advantage of considering this state transformation is 
that the new unforced system dynamics becomes highly sim- 
plified as time-invariant, although the system output becomes 
time-varying and it is still nonlinear 

± x (t) = -x 2 (i)-u(t), 

x 2 (i) = 0, (8) 
Pi (t) = WK-T^^x^l i = l,...,n r , 
where u(t) = H(t)v T (t). 

B. State augmentation 

In order to derive a linear system that mimics the dynam- 
ics of the original nonlinear system, a state augmentation 
procedure follows inherited directly from the kinematics of 
the nonlinear range outputs of ([8]). Thus, taking the time- 
derivative of Pi(t) in |8} yields 

PS) = ^ [(b?S(u;(t))K T (t) - u T (i)) Xl (t) 
+bf K T {t)x 2 (t) - Xl T (t)x 2 (t) + bf K T (t)u(t)] . (9) 



Identifying the nonlinear part x 1 T (i)x 2 (i) in (|9]l leads 
to the creation of the augmented states that will mimic 
this nonlinearity: x nr+3 (t) :— x 1 T (t)x 2 (t) and x nr+i (t) :— 
||x 2 (t)|| 2 , with the corresponding kinematics x„ r+3 (t) — 
— u T (i)x 2 (i) — x nr+i and x nr+i (t) = 0. 

Thus a new dynamic system is created by augmenting the 
original nonlinear system with the states 



x 3 (t) •= Pi(t), 



,(*) := Xl T (t)x 2 (t), 



x nr+2 (t) := p nr (t), 
x nr+i {t) := ||x 2 (£)|| 2 , 



and denoting the new augmented state vector x(i) e R 8 +™>' 
by 

x(t) = [x! T (t) x 2 T (i) x 3 (t) . . . x nr+2 (t) x nr+3 (t) x nr+i (t)] T . 



Combining the new augmented states dynamics with <(8j it 
is easy to verify that the augmented dynamics can be written 

as 

x(i) = A(i)x(i) +B(i)u(i), 

where 



A(t) 








(bfSMt))TC T (t)~ 



(t)) 



Pl(*) 



-I 



pi(t) 










(b£ 5(w(t))K T (t)-u T (t)) n T (t) 



PnAt) 





— 

Pn r (t) 

-u T (t) 








1 

'pi(i) 



pnAt) 










-1 


(10) 



actually measured, it is straightforward to show from the 
outputs of (|6]l that 



which leads to 

2(b i -bj) T R r (t)x 1 (t 

or, equivalently 



2(b< 



^ T (t) Xl (t), 



+ P*(t) 



, v _ llb^f-Hb,!! 2 



2(b i -b 3 ) r TC T (t)x 1 (t) / ■ \ , v 

Pi(*)+Pj(*) t-tj +'W *2+jW 



Pi{t)+P 3 {t) ' 

(12) 

where the right hand-side of ( fT2} is measured and the left 
hand-side depends on the system state. 

In order to complete the augmented system dynamics, 
discarding the original nonlinear outputs in ([8]), and con- 
sidering ( p~2] >, define the new augmented system outputs 

y(*)e 



nn r +nc 



y(*) = 



[x 3 (t) x 3 (t)-x 4 (t) ■ 

2(b 1 -b 2 ) T TC T (t)x 1 (t) 
Pl(t)+pi(t) 



x 3 {t) - x 2+nr (t)\ 



x 2+1 {t) 



>(*) 



Pl(t) + P3(«) 



~t~ ^2+l(^) ^2+3 (^) 



2(b„ r _ 2 -b„ r ) T TC T (t)x 1 (t) 
p nr -2(t)+p (t) 

2(b„ r _i~b„ r ) r R T (t)xi(t) 
p„ r -l(t)+p„ r (t) 



_ 3 (t) - a; 2+ n r (*) 

-l(*) - ^2 + n r (t). 



is the number of 



and 



where n c = C 2 = 2( „;_ 2) , - 2 ' 
all possible 2-combinations of n r elements. 

In compact form, the augmented system dynamics can be 
written as 



B(i) 



piW 



n{t)h nr 

Pn r (t) 







(11) 

The following assumption is required so that ( fT0] > and ( |TT[ > 
are well defined. 
Assumption 1: The motion of the vehicle is such that 



3 



Rrt 



<P,{t)<Rn 



V 

* >t 

% 1 ^ ■ ■ ■ J T\>i^ 

From a practical point of view this is not restrictive since 
the vehicle and the coupled array will never be on top of a 
transponder, and neither will the ranges converge to infinity. 

Note that the RDOA at the receivers are considered to be 
measured more accurately compared to the absolute distance 
between the transponder and any given reference receiver of 
the USBL. Selecting a reference sensor on the array, for 
instance receiver 1 for now, all the other ranges are easily 
reconstructed from the range measured at receiver 1 and the 
RDOA between receiver 1 and the other receivers, that is 
PM = PM - 6 Pi j(*)> where 8pij(t) = Pi{t) - p 3 (t). 

Taking into account that the augmented states 
x 3 (t), . . . ,x nr+2 (t) that correspond to the ranges are 



x(t) = A(i)x(t) + B(i)u(i), 
y(t) = C(t)x(t), 



(13) 



where 



C(t) = 



Co = 



1 o 
i -l 



and 



0„ rX3 0„ 

Ci(t) o n 



Co 0„ r 

c 2 o„ r 



c 2 = 



-10 

o-io 



10 

o o 1 



d(t) = 



2(b 1 -b 2 ) r TC T (t) 

Pl(*) + P2(t) 

2(b 1 -b 3 ) r 7^(t) 



2(b„ 



-b, lr ) T K T (t) 



p nr -2(t)+ Pnr (t) 
2(b„ r _i-b„ r ) T R T (t) 

p nr -l(t)+Pn r (t) 



C. Observability analysis 

The Lyapunov state transformation and the state aug- 
mentation that were carried out allowed to derive the LTV 
system described in ( |13) , which ensembles the behavior of 
the original nonlinear system |6]). The dynamic system ( p~3] > 
can be regarded as LTV, even though the system matrix 
A(t) depends explicitly on the system input and output, as 
evidenced by ( fTO) . Nevertheless, this is not a problem from 
the theoretical point of view since both the input and output 
of the system are known continuous bounded signals. The 
idea is not new either, see, e.g., [9], and it just suggests, in 
this case, that the observability of ( fT3) l may be connected 
with the evolution of the system input or output (or both), 
which is not common and does not happen when this matrix 
does not depend on the system input or output. 

In order to fully understand and couple the behavior of 
both systems, the observability analysis of ( fT3| l is carried out 
in this section, using classical theory of linear systems. This 
analysis is conducted based on the observability Gramian 
associated with the pair (A(t), C(t)), which is given by [10] 



W(to,t f )= / $ J (Mo)C J (i)C(t)$(i,i )^, 

J t 

where to) is the state transition matrix of the LTV 
system ([13]). Let 



u il] {t,t ) = f u(a)da 

J t 



Tedious, lengthy, but straightforward, computations show 
that the transition matrix associated with A(t) is given by 



*(t,*o) 



$AA(t,to) 6x 

$ba(Mo) I„ 
$ca(Mo) 2x 



^6x2 

$bc(Mo) 
®cc(t,t ) 



where 



$AA(t,t a ) = 



I -(t-to)I 
I 



®BA(t,t ) = [$BAl(Mo) ®BA2{t,t 
J to 



$BAl(t,to) 



ho — = d,J 



$BAa(*,*o) = 

rt b^ ( ) t u^to) da 

u)(<r))' " 
Pi( CT ) 



rt -(a-t Q )(h1S(u,(a))-R T (a)- U T (a)) ^ 
Jt 



r' 



P™,» d<J_r Jto Pn r M 



pt u 

Jtn ~ 



[i]T 



+ L 7"^ Of 



$Bc(Mo) = [$BCl(Mo) 



to Pi(o-) 



<7<7 



$BC2(*,*o)] 
J to Pi(c) 



r^tol^) d(J /< 



to P>i r (ff) 



da 



(14) 



and $c?A(i, to) and $cc(f,< ) are omitted as they are not 
required in the sequel. 

Before proceeding with the observability analysis, the fol- 
lowing assumption is introduced which ultimately asserts the 
minimal number of receivers and configuration requirements 
of the USBL array in order to render the system observable 
regardless of the trajectory described of the vehicle. 

Assumption 2: There are at least 4 non-coplanar receivers. 

The reasoning behind the need to have at least 4 non- 
coplanar receivers is that this is the minimal configuration 
that guarantees the uniqueness for the transponder position 
r(t). 

The following theorem establishes the observability of the 
LTV system ( [13) . 

Theorem 1: The linear time-varying system ( fT3| l is ob- 
servable on [to,tf], to < tf. 

Proof: The observability proof of the LTV system ( p~3] > 
is accomplished by contradiction. Thus suppose that (jT3j is 
not observable on 1 := [t , tf]. Then, there exists a non null 
vector d <E 



p8+n r 



d=[d^ 



with di £ R 3 , d 2 £ R 3 , d 3 £ R n \ d 4 ,d 5 £ K 
d T W(< , tf)d = for all t £ X , or equivalently, 



f ||C(r)<i>(T,<o)d|| 2 dT = 0, V 

Jt 

Taking the time derivative of ( fT6) i gives 

C(*)$(Mo)d = 0, Vtgz. 

From (Tf) , at t = to comes 

C d 3 _ 
C 1 (t )d 1 + C 2 d 3 U ' 



tei- 



(15) 
such that 

(16) 
(17) 
(18) 



which immediately implies that Cod 3 = 0. As Co is not 
singular allows to conclude that the only solution is the null 
vector 

d 3 = 0. 



(19) 



Replacing ( fT9| > in ([18} yields 

1 rflh-b,) 3. 



Pl(to)+P2(t ) 1 

2 

Pl(to)+P3(to) 



(b 1 -b 3 ) J 



.-2(to)+P„ r (to)( b "-- 2 b " 



^ T (t )di = 0. (20) 



.Pn r -l(t )+Pn r (*0) 1 

Under Assumption [2] the only solution for ( |20] i is 
di = 0. 



(21) 



From fPT} comes that 

C <S>BA2(t,to)d 2 



Co® sett, t Q ) 



(22) 



(t-t )(bf5(^(t))R T (t)-u T (t)) 



Taking the time derivative of (|22]i allows to write 



Co 



pi(t) 



[i]T 



W (t) Q 2 ~ r Pl (t) 

(*~«o)(b^ r .S(u;(t))K r (t)-u r (t)) 
P» r (t) 

b^^(t) d2 ^uW T (i,t )^. 



^4 

(Mo) j , (t-t )rf 5 

d 2 + -^*r 



-(*) 



-(*) 



: P» r (t) 

(t-t )rf 5 

p»,(t) 



(23) 



Evaluating f23| > at t = to yields 

-1 

Pi (to) Pi (to) 

Co ' 



AM 



•(to). 



ft T (*o)d 2 



(24) 



Taking into account that C is not singular, it is easy to 
verify that under Assumption [2] the only solution for p4| is 



pi(to) PI (to) 



p n ,(to) 



C?4 







0. (25) 



P» r (to)_ 

Now setting ([19), |2T}, and (|25j in (|23j yields 



Co 



t-to 
Pi (to) 



t-to 



= 0. 



(26) 



Again the only possible solution for ( |26] > is 

rf 5 = 0. 

This concludes the proof since the only solution d = of 
( |T6] > contradicts the hypothesis of the existence of a non null 
vector d such that ( fTo*) is true. Thus, by contradiction, the 
LTV system ( fLT) is observable. ■ 

Although the observability of the LTV system ( fTJ) has 
been established, it does not mean that the original nonlinear 
system |6) is also observable, and neither means that an 
observer for ( fTJ) is also an observer for |6). This however 
turns out to be true, as it is shown in the next theorem. 

Theorem 2: The nonlinear system <|8j is observable in the 
sense that, given {y(t),t £ [t ,tf]} and {u(i),f € [io,i/]}, 
the initial state x(io) = [ x i"(^o) x^io)] is uniquely 
defined. Moreover, a state observer for the LTV system ( p"3j ) 
with globally asymptotically stable error dynamics is also 
a state observer for the nonlinear system ([8]), with globally 
asymptotically stable error dynamics. 

Proof: The observability of the LTV system ( fT3) has 
already been established in Theorem [T] thus given {y(t),t € 



[t ,tf]} and {u(t),t € [*0j*/]}> tne initial state of ( fT3) is 
uniquely defined. Let 

z(t ) = [zf(t ) zj(i ) zj (t ) z 4 (t Q ) z 5 (to)] T , 

with Zl (to),z 2 (io) £R 3 , z 3 (t ) eK Tl %andz 4 (io)^5(<o) € 
K 3 be the initial state of the LTV system ([13]) and 

x(t ) = [xf(to)xJ(i )] T , 

be the initial state of the nonlinear system ([8]). The evolution 
of xi (t) for the nonlinear system |8) can be easily shown to 
be given by 



Xi (t) = Xi(to) - (t - t )x 2 (to) - u [11 (t,i ), 



(27) 



which is similar to the evolution of xi (t) for the LTV system 
differing only in the initial condition. Using ( |2"7) , the output 
of the nonlinear system ([8]) can be shown to satisfy 

p 2 {t) =||xi(i ) - ^(t )b,|| 2 - 2xf (t )K{t)b t 

+ (t- t ) 2 ||x 2 (<o)|| 2 + 2xf (to)ft(*b))b, 
- 2(i - i )x 2 r (t )xi(i ) + 2(< - f )x 2 r (t )^(<)b ! 
+ 2(f - i )x 2 r (to)u [11 (i,i ) ~ 2u [llT (t,i )x 1 (i ) 
+ 2u [I]T (MoTOb i + ||u [11 (i,t )|| 2 , (28) 

and the squared range difference between receiver i and j 

p 2 (t) - p 2 3 {t) = 2(b, - b J ) T (7e T (to)x 1 (t ) - 7e T (t) Xl (t)) 
+ | Xl (t ) - H{t )bA\ 2 - || Xl (t ) - ^(to)^!! 2 . (29) 

Now, multiplying the set of augmented outputs of the LTV 
system ( fT3j ) by the corresponding sum of pair of ranges, 
and taking into account that the states x 3 (t), . . . ,x 2+ „ r (t) 
correspond to the actual ranges Pi(t), ... ,p nr (t), gives that 

^ (b< " P X TC J(t) )xlW + x ^ - + P'®) 

= 2(b 4 - h 3 ) T n T {t)^{t) + p- (t) - p?(*)- (30) 

The squared range of the LTV system ( fT3j ) in ([30) can be 
shown to satisfy 

P?(t) =4+i(to)- 2zJ(t )U(t)b i 

+ (t- to) 2 z rir+i {to) + 2zf(to)^(* )b 4 
- 2(t - t )zn r+3 (io) + 2(i - i )z 2 r (io)^Wb ! 
+ 2(t - t )2% (t W 1] (t, t ) - 2u [1] T (t, < )zi (t ) 
+ 2u^ T (t,to)K(t)b z + \\u^(t,t )\\ 2 , (31) 

and consequently it is true, for the LTV system ( p"3) , that 

P 2 (*)-P-(*) = 4+i(*o)-^ +j (*o) 
- 2(b, - b,) T ft T (i) (zi(t ) - (t - io)a£(t ) - uW(t,io)) 
+ 2(b 1 -b J ) T 7e T (t )zi(i ). (32) 

From the LTV system ( fTJ) transition matrix and forced 
response comes 



xi (t) = zi(t ) - (t- i )z 2 (io) - u [11 (Mo). 



(33) 



Replacing ((33]) and Q in ([30) yields 

2(b, - b,) T ft T (t) Xl (t) + p*(t) - p%t) = 

4h(*o) - 4 +j ^o) + 2zf (t )^(t )(b i - b 3 .). (34) 

The augmented states of the LTV system (jT3j X2+i(t), 
i = 1, ... , n r , are actually considered to be measured and 
correspond to the range measurements. Therefore, it must be 
for its initial condition 



z 2+l (t Q ) = ||xi(t )-ft(t )b 4 ||. 



(35) 



Now taking ( |33j > into account, and comparing the evolution 
of the augmented outputs ( f34] > for the LTV system with ( |29| ) 
for the nonlinear system, it follows that 



where 



B c ft T (i )[xi(i )-zi(io)] =0, 



(b x - b 2 ) T 
(b, - b 3 ) T 



(36) 



(b„ 
,(b„ 



- a -b Br ) T 

-,-b^r. 



Under Assumption [2] the only solution of ( po*) is 

Xi(*o) = Zi(*o)- 



(37) 



Setting ( [35) and ( |3"7) and comparing the difference between 
square of ranges for both systems yields 

2(f - t )(b 4 - b,) T ^ T (t) [x 2 (to) - z 2 (t )] - 0. (38) 

Taking the time derivative of ([38) comes 

[-2(t-t ){b i -b,) T S{uj(t))'R T (t) 

+2(b, - h 3 ) T K T (t)} (x a (i ) - z 2 (to)) = 0. (39) 
At t = to, ((39) comes as 

B c K T (t Q ) [x 2 (t ) - z 2 (t )] = 0. (40) 
Again under Assumption [2] the only solution of ( |40) is 

x 2 (t ) =a a (*o). (41) 

Finally, setting (|35), (|37), and ( |4"T) in ( f3Tj ) and comparing to 
|28) yields 

- 2(t - t ) (xSf (to)xi(to) - z nr+3 (t )) 

+ (t - t ) 2 (||x 2 (t )|| 2 - 2„ r+4 (io)) = 0. (42) 

As (t — to) and (t — to) 2 are linearly independent functions, 
the only solution for (|42) is z nr +3(to) = x 2 n (t )x 1 (t ) 
and Zn r +4(to) = ||x 2 (to)|| 2 . Thus, the initial state of the 
nonlinear system ([8) matches the initial state of the LTV 
system ( fT3) which is uniquely defined. Therefore the non- 
linear system (|8) is also observable. ■ 
Note that the usual concept of observability for nonlinear 
systems is not as strong as that presented in the statement 
of Theorem [2] see [11]. Also, in the observer structure 
that was derived, there is nothing imposing the nonlinear 
algebraic relations between the original and the additional 



states, allowing for the dynamics of the new system to be 
considered linear time-varying. It is however trivial to show 
that those relations are indeed preserved asymptotically. 

Although the observability results were derived with re- 
spect to the nonlinear system ([8), they also apply to the 
original nonlinear system |6) as they are related through a 
Lyapunov transformation. Thus, the design of an observer for 
the original nonlinear system follows simply by reversing the 
state transformation |7), as it will be detailed in the following 
section. 

D. Kalman filter 

The observer structure devised so far was based on a 
deterministic setting providing strong constructive results, 
in the sense that it was shown, in Theorem [2] that an 
observer with globally asymptotically stable error dynamics 
for the LTV system ([13) provides globally asymptotically 
stable error dynamics for the estimation of the state of the 
original nonlinear system. However, in practice there exists 
measurement noise and system disturbances, motivating the 
derivation of a filtering solution within a stochastic setting. 
Therefore, the design of a LTV Kalman Filter (even tough 
other filtering solutions could be used, e.g. a T-Lx, filter) is 
presented next. Before proceeding with the derivation of the 
proposed filter, it is important to stress, however, that this 
filter is not optimal, as the existence of multiplicative noise 
is evident by looking into the LTV system matrices. 

Nevertheless, the errors associated with the Kalman filter 
estimates are GAS, as it can be shown that the system is not 
only observable but also uniformly completely observable, a 
sufficient condition for the stability of the LTV Kalman filter 
[12]. The following assumption and lemma are introduced to 
guarantee the uniform complete observability of the system. 

Assumption 3: The position of the transponder in the 
vehicle coordinate frame r(t), and the angular and linear 
velocities, w(t) and v(t) respectively, are bounded signals. 
Moreover, the time derivatives of these signals (r(t), cl>(t), 
and v(t) respectively), are also bounded, as well as the 
derivatives of the ranges p ; (t), with i = 1, . . . , rt r . 

Lemma 3 ([13, Lemma 1]): Let f(t) :p ,i/]cl-> E™ 
be a continuous and two times continuously differentiable 
function on I := [t ,t/], T := tf — to > 0, and such 
that f (to) = 0. Further assume that maxt e x ||f (f)|| < C. 
If there exists a constant a* > and a time t* € I such 
that ||f(t*)|| > a*, then there exists constants (3* > and 
< S* < T such that ||f (t + 8*) || > /3*. 

From a practical point of view, Assumption [3] is not 
restrictive since the systems presented herein are in fact finite 
energy systems that ensemble realizable physical vehicles 
and sensors. The LTV system ( fT3| ) is finally shown to be 
uniformly completely observable in the following theorem. 

Theorem 4: The linear time-varying system ( fT3) is uni- 
formly completely observable, that is, there exists positive 
constants a±, a 2 , 6, such that o^I ^ W(t, t + S) ^ a 2 I 
for all t > t . Proof: The bounds on the observability 
Gramian W(t, t + S) can be written as 



a x < d T W(t,t + S)d < a 2 , 



(43) 



for all t > t , and for all d € M. 8+n *- such that ||d|| = 1. 
The proof follows by noticing that (j43j can be written as 



ol\ < J t * + || f (r) || 2 e?r < a 2 , where 

f(r) := C(r)$(T,£)d. 



(44) 



The existence of the upper bound a 2 is trivially checked, as 
under Assumption [3] the matrices A(t) and C(i) are norm- 
bounded and f(r) is integrated over limited intervals. Let 
d = [dl dl dl di d 5 ], with di G R 3 , d 2 G R 3 , 
d 3 G E" r , d 4 ,d 5 G E. Evaluating (04]) at r = t, it 
is straightforward to verify that if d 3 7^ 0, then ||f(t)|| 
is immediately bounded. Indeed, as ||f(t)|| > || Cod3 1| = 
a\ > for all t > to, since Co has full column rank by 
construction. Suppose now that d 3 = 0. Then, it can also 
be seen that if dx ^ 0, it is true that ||f(t)|| = ||Ci(i)d 1 ||, 
which is clearly bounded by 



f(t)|| > ( T mm (D7^(t))||2C 2 U r ^ T (i)d 1 ||, 



(45) 



for all t > to, where the operator a m i n (A) represents the 
smallest singular value of A and 



sn r x3 



T> p+ (t) := diag 



Pi(t)+ P2 (t) 
pi(t)+ P3 (t) 



p nr -2(t)+ Pnr (t) 
p nr -l(t)+p nr (t) 



Under Assumption 1 it is clear that a min (D p ^(t)) > 
for all t > to, whicn implies 



|f(*)ll > 



-CTmin(C 2 U r )||7e T (t)di| 



for all t > to- Now under Assumption|2]it follows that C2U r 
has full column rank and therefore there exists a positive 
constant f3{ such that <7 TO j n (C 2 U r ) = (3{ > 0. Thus, taking 



into account that \\1Z (t)di 



\{(t)\\ > 



|di||, 



Idxll = at, > 



(46) 



for all t > to- Using the same set of assumptions and 
procedures it is possible to show that the derivative of f(r) 
evaluated at t = t is also uniformly bounded 



9f(r) 




> P2^3 


"d 2 " 




dr 


T=t 


Rmax 


C?4 





at > 0, 



(47) 



for all t > to, with $\ and /3 3 positive constants, and when 
d 3 = 0, di = and either d2 ^ or d^ ^ 0. The upper 
boundedness on the norm of the second derivative of f(r) 
becomes straightforward under Assumption [5] thus allowing 
the use of Lemma [3] in ( |47j ). Thus, in this case, it can be 
shown, using Lemma [3] that there exist a\ > and 5* > 
such that ||f(i + 5l)\\ > a\ for all t > t . Using Lemma [5] 
again, there exist positive constants al > and 5* > such 
that 

d T W{t,t + 5*)d > a* 5 , (48) 



for all t > to and when d 1 — 0, d 3 = 0, and d 2 7^ or 
c?4 7^ 0. When all the components of d are null except d$ it 
follows that 



C $bc2(t-, t)d 5 
C 2 $bc2(t-, t)d 5 



f(r) = 

which norm is clearly bounded by 

||f(r)|| > a mm {C )\\3> BC 2(T,t)d 5 \\ 
for all t > to- Expanding |50| and using ( fT4] > yields 



|f(r)|| > /? 2 * 



G - t 



-da 



(49) 



(50) 



(51) 



for all t > to- In particular at t = with 5| > 0, comes 



\f(t + 6* 2 )\\>(3* 2 \d 5 \ 




t+Si 



a - t 



da 



(52) 



for all t > to- By the Integral Mean Value theorem there 
exists c E]t, t + 5 2 [ such that 



t+s* 



da — St 



(53) 



for all t > to- Now defining <5 3 := c — t > 0, which is clearly 
positive because c > t, allows to write 



jt+s* 



a - t 



-da 



°2°3 



Pl (a) Piit + 81) 



(54) 



for all t > to- Under Assumption [T] comes from (j52j) and 
34l that 



\f(t + S* 2 )\\>f3 2 \d 5 



\ 



E 

»=i 



Rn 



al > (55) 



for all t > to- Finally, Lemma [3] is used once again to show 
that there exist a > and S > 0, for all t > to and {d G 
R 8+n r . || d || = ^ such mat d r W(t,i + S)d > a, which 

means that the system is uniformly completely observable 
and therefore concludes the proof. ■ 

To recover the augmented system dynamics in the orig- 
inal coordinate space, the original Lyapunov state trans- 
formation |7| is reverted considering the augmented state 
transformation T(t) := T r (i)x(t), where T r (i) := 
diag (lZ T (t), TZ T (t), 1, . . . , l) is a also Lyapunov state trans- 
formation that preserves all observability properties of the 
LTV system ftl) . 

Including system disturbances and sensor noise yields the 
final reverted augmented dynamics 

f f (t) = A r (t)r(t) + B r (t)v r (t) + n x (t), 
I y(t) = Cr(t)r(«)+n y (t), 



where 



A r (t) = 

-S(u(t)) 


b^5(u;(t))-v r r (t) 
Pi(*) 

b^5(w(t))-v r T (t) 





-I 

-5(w(t)) 

pi(t) 







P« r (*) 





















1 

'pl(t) 







B r (t) - 
C r (t) 



bi 

Pi(*) 



o„ 



o„ 



Ci(t)^(t) 0„ 



Co O n 

c 2 o„ 







where n x (i) and n y (t) are assumed to be uncorrelated, 
zero-mean, white Gaussian noise, with E [n x (i)n x T (T)] = 
Q x (t)6(t - t) and E [n y (f)n y T (r)] = Q y (t)S(t - r). 

IV. Simulation Results 

The performance of the proposed filter was assessed 
in simulation using a kinematic model for an underwater 
vehicle. The vehicle describes a typical survey trajectory as 
depicted in Figure [2] 




The range measurements between the transponder and the 
reference receiver (receiver 1) are considered to be disturbed 
by additive, zero-mean white Gaussian noise, with 1 m 
standard deviation whilst the RDOA between receiver 1 and 
the other 3 receivers is considered to be measured with an 
accuracy of 6 mm. The transponder is located in inertial 
coordinates at 7 p t = [200 0] [m], and the unknown 
underwater current velocity has an intensity of 0.2 m/s 
in all three axis. The augmented states that correspond to 
the ranges x 3 , . . . , x 2+ „ r are initialized with the first set 
of measurements available, the filter position estimate is 
initialized with an offset of 20 m from the nominal position, 
and the rest of the initial estimates is set to zero. 

The initial evolution of the position and current velocity 
estimation error is depicted in Figure [3] The full evolution 
of the augmented states error is represented in Figure |4] The 
attainable performance of the filter is better illustrated in 
Figure [5] where the steady-state response of the position and 
current velocity estimation error is shown. 




20 40 



100 120 140 
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Fig. 3. LTV Kalman filter initial convergence 



Fig. 2. Vehicle nominal trajectory 



The USBL receiving array is composed of 4 receivers 
that are installed on the vehicle with an offset of 30 cm 
along the x-axis of the body-fixed coordinate frame {B} 
where the DVL and the rate gyros are also installed. Thus 
the positions of the receivers with respect {B} are given in 
meters by b ± = [0.2 -0.15 0] T , b 2 = [0.2 0.15 0] r , 
b 3 = [0.4 0.15] T , and b 4 = [0.4 -0.15] T . 

The DVL fluid-relative velocity measurements are con- 
sidered to be corrupted by additive uncorrelated zero-mean 
white Gaussian noise with an accuracy of 0.2% of the 
nominal velocity with an additional standard deviation of 
1 mm j ' s, which is inspired on the LinkQuest NavQuest 600 
Micro DVL sensor package. The rate gyros are also inspired 
on a realistic sensor package, the Silicon Sensing CRS03 
triaxial rate gyro, and are thus considered to be disturbed by 
additive, uncorrelated, zero-mean white Gaussian noise, with 
a standard deviation of 0.05 deg/s. 




Fig. 4. LTV Kalman filter augmented states evolution 

The performance of the filter is compared with two alterna- 
tive filter designs, the well-known and established Extended 




Us] 



Fig. 5. LTV Kalman filter steady state response 



Kalman Filter (EKF) and the Kalman filter with the Planar- 
Wave approximation (KFPW). The first design linearizes the 
nonlinear range and RDOA measurements about the filter 
estimates in order to compute a suboptimal Kalman gain. 
In the latter, the feedback is accomplished by means of a 
precomputed transponder position fix from the USBL that 
resorts to a planar-wave approximation, previously used by 
the authors [6]. 

The Root-Mean-Square (RMS) of the steady-state position 
error of the three filters is presented in Table [I] Comparing 
the steady-state response of the three filters it can be seen that 
all of them attain the same performance level. The solution 
presented in this work has the advantage of being GAS, 
which is not guaranteed for the other designs. 

TABLE I 

Steady-state transponder position r(t) error RMS comparison 



similar performance levels using realistic sensor noise and 
disturbances. The advantage of the new filter structure is 
nevertheless evident, due to its GAS properties which is not 
guaranteed for either of the two traditional solutions. 
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Filter x [m] y [m] z [m] 

LTV Kalman^ 0.0880 0.0837 OTTToT 

KFPW - 0.1122 0.0847 071096 

EKF O06TI 0.0814 0.1128 



V. Conclusions 

The main contribution of the paper lies on the design of 
globally asymptotically stable position filters based directly 
on the nonlinear sensor readings of USBL and a DVL. At the 
core of the proposed filtering solution is the derivation of a 
LTV system that fully captures the dynamics of the nonlinear 
system. The LTV model is achieved through appropriate state 
augmentation allowing for the use of powerful linear system 
analysis and filtering design tools that yield GAS filter error 
dynamics. 

The performance of the proposed filter was assessed in 
simulation and compared against two traditional solutions, 
the EKF and a Kalman filter that resorts to the planar 
approximation of the acoustic wave arriving at the USBL 
array. Comparison of the steady-state position error from the 
three designs lead to the conclusion that all demonstrated 



